function H_Pos_Relative = calcH_Pos_Relative(param,states_old,states_new)

% x(k)
lamda_old = states_old(25);

q0_wi_old = states_old(1);
q1_wi_old = states_old(2);
q2_wi_old = states_old(3);
q3_wi_old = states_old(4);

q0_vw_old = states_old(36);
q1_vw_old = states_old(37);
q2_vw_old = states_old(38);
q3_vw_old = states_old(39);

px_iw_old = states_old(8);
py_iw_old = states_old(9);
pz_iw_old = states_old(10);

px_ci_old = states_old(26);
py_ci_old = states_old(27);
pz_ci_old = states_old(28);

px_vw_old = states_old(33);
py_vw_old = states_old(34);
pz_vw_old = states_old(35);

% x(k+m)
lamda_new = states_new(25);

q0_wi_new = states_new(1);
q1_wi_new = states_new(2);
q2_wi_new = states_new(3);
q3_wi_new = states_new(4);

q0_vw_new = states_new(36);
q1_vw_new = states_new(37);
q2_vw_new = states_new(38);
q3_vw_new = states_new(39);

px_iw_new = states_new(8);
py_iw_new = states_new(9);
pz_iw_new = states_new(10);

px_ci_new = states_new(26);
py_ci_new = states_new(27);
pz_ci_new = states_new(28);

px_vw_new = states_new(33);
py_vw_new = states_new(34);
pz_vw_new = states_new(35);


%% Construct position H matrix in relative measurement

%% pH(k)

% pHk(q_wi) = delta(z_p_relative) / delta(q_wi_k)
H_z_p_relative_qwi_old = [[ -lamda_old*((2*q0_vw_old*q2_vw_old + 2*q1_vw_old*q3_vw_old)*(2*py_ci_old*q1_wi_old - 2*px_ci_old*q2_wi_old + 2*pz_ci_old*q0_wi_old) - (2*q0_vw_old*q3_vw_old - 2*q1_vw_old*q2_vw_old)*(2*px_ci_old*q3_wi_old + 2*py_ci_old*q0_wi_old - 2*pz_ci_old*q1_wi_old) + (2*px_ci_old*q0_wi_old - 2*py_ci_old*q3_wi_old + 2*pz_ci_old*q2_wi_old)*(q0_vw_old^2 + q1_vw_old^2 - q2_vw_old^2 - q3_vw_old^2)), -lamda_old*((2*q0_vw_old*q3_vw_old - 2*q1_vw_old*q2_vw_old)*(2*py_ci_old*q1_wi_old - 2*px_ci_old*q2_wi_old + 2*pz_ci_old*q0_wi_old) + (2*q0_vw_old*q2_vw_old + 2*q1_vw_old*q3_vw_old)*(2*px_ci_old*q3_wi_old + 2*py_ci_old*q0_wi_old - 2*pz_ci_old*q1_wi_old) + (2*px_ci_old*q1_wi_old + 2*py_ci_old*q2_wi_old + 2*pz_ci_old*q3_wi_old)*(q0_vw_old^2 + q1_vw_old^2 - q2_vw_old^2 - q3_vw_old^2)),  lamda_old*((2*q0_vw_old*q2_vw_old + 2*q1_vw_old*q3_vw_old)*(2*px_ci_old*q0_wi_old - 2*py_ci_old*q3_wi_old + 2*pz_ci_old*q2_wi_old) + (2*q0_vw_old*q3_vw_old - 2*q1_vw_old*q2_vw_old)*(2*px_ci_old*q1_wi_old + 2*py_ci_old*q2_wi_old + 2*pz_ci_old*q3_wi_old) - (2*py_ci_old*q1_wi_old - 2*px_ci_old*q2_wi_old + 2*pz_ci_old*q0_wi_old)*(q0_vw_old^2 + q1_vw_old^2 - q2_vw_old^2 - q3_vw_old^2)),  lamda_old*((2*q0_vw_old*q3_vw_old - 2*q1_vw_old*q2_vw_old)*(2*px_ci_old*q0_wi_old - 2*py_ci_old*q3_wi_old + 2*pz_ci_old*q2_wi_old) - (2*q0_vw_old*q2_vw_old + 2*q1_vw_old*q3_vw_old)*(2*px_ci_old*q1_wi_old + 2*py_ci_old*q2_wi_old + 2*pz_ci_old*q3_wi_old) + (2*px_ci_old*q3_wi_old + 2*py_ci_old*q0_wi_old - 2*pz_ci_old*q1_wi_old)*(q0_vw_old^2 + q1_vw_old^2 - q2_vw_old^2 - q3_vw_old^2))]
                          [ -lamda_old*((2*q0_vw_old*q3_vw_old + 2*q1_vw_old*q2_vw_old)*(2*px_ci_old*q0_wi_old - 2*py_ci_old*q3_wi_old + 2*pz_ci_old*q2_wi_old) - (2*q0_vw_old*q1_vw_old - 2*q2_vw_old*q3_vw_old)*(2*py_ci_old*q1_wi_old - 2*px_ci_old*q2_wi_old + 2*pz_ci_old*q0_wi_old) + (2*px_ci_old*q3_wi_old + 2*py_ci_old*q0_wi_old - 2*pz_ci_old*q1_wi_old)*(q0_vw_old^2 - q1_vw_old^2 + q2_vw_old^2 - q3_vw_old^2)),  lamda_old*((2*q0_vw_old*q1_vw_old - 2*q2_vw_old*q3_vw_old)*(2*px_ci_old*q3_wi_old + 2*py_ci_old*q0_wi_old - 2*pz_ci_old*q1_wi_old) - (2*q0_vw_old*q3_vw_old + 2*q1_vw_old*q2_vw_old)*(2*px_ci_old*q1_wi_old + 2*py_ci_old*q2_wi_old + 2*pz_ci_old*q3_wi_old) + (2*py_ci_old*q1_wi_old - 2*px_ci_old*q2_wi_old + 2*pz_ci_old*q0_wi_old)*(q0_vw_old^2 - q1_vw_old^2 + q2_vw_old^2 - q3_vw_old^2)), -lamda_old*((2*q0_vw_old*q3_vw_old + 2*q1_vw_old*q2_vw_old)*(2*py_ci_old*q1_wi_old - 2*px_ci_old*q2_wi_old + 2*pz_ci_old*q0_wi_old) + (2*q0_vw_old*q1_vw_old - 2*q2_vw_old*q3_vw_old)*(2*px_ci_old*q0_wi_old - 2*py_ci_old*q3_wi_old + 2*pz_ci_old*q2_wi_old) + (2*px_ci_old*q1_wi_old + 2*py_ci_old*q2_wi_old + 2*pz_ci_old*q3_wi_old)*(q0_vw_old^2 - q1_vw_old^2 + q2_vw_old^2 - q3_vw_old^2)),  lamda_old*((2*q0_vw_old*q3_vw_old + 2*q1_vw_old*q2_vw_old)*(2*px_ci_old*q3_wi_old + 2*py_ci_old*q0_wi_old - 2*pz_ci_old*q1_wi_old) + (2*q0_vw_old*q1_vw_old - 2*q2_vw_old*q3_vw_old)*(2*px_ci_old*q1_wi_old + 2*py_ci_old*q2_wi_old + 2*pz_ci_old*q3_wi_old) - (2*px_ci_old*q0_wi_old - 2*py_ci_old*q3_wi_old + 2*pz_ci_old*q2_wi_old)*(q0_vw_old^2 - q1_vw_old^2 + q2_vw_old^2 - q3_vw_old^2))]
                          [ -lamda_old*((2*q0_vw_old*q1_vw_old + 2*q2_vw_old*q3_vw_old)*(2*px_ci_old*q3_wi_old + 2*py_ci_old*q0_wi_old - 2*pz_ci_old*q1_wi_old) - (2*q0_vw_old*q2_vw_old - 2*q1_vw_old*q3_vw_old)*(2*px_ci_old*q0_wi_old - 2*py_ci_old*q3_wi_old + 2*pz_ci_old*q2_wi_old) + (2*py_ci_old*q1_wi_old - 2*px_ci_old*q2_wi_old + 2*pz_ci_old*q0_wi_old)*(q0_vw_old^2 - q1_vw_old^2 - q2_vw_old^2 + q3_vw_old^2)),  lamda_old*((2*q0_vw_old*q1_vw_old + 2*q2_vw_old*q3_vw_old)*(2*py_ci_old*q1_wi_old - 2*px_ci_old*q2_wi_old + 2*pz_ci_old*q0_wi_old) + (2*q0_vw_old*q2_vw_old - 2*q1_vw_old*q3_vw_old)*(2*px_ci_old*q1_wi_old + 2*py_ci_old*q2_wi_old + 2*pz_ci_old*q3_wi_old) - (2*px_ci_old*q3_wi_old + 2*py_ci_old*q0_wi_old - 2*pz_ci_old*q1_wi_old)*(q0_vw_old^2 - q1_vw_old^2 - q2_vw_old^2 + q3_vw_old^2)),  lamda_old*((2*q0_vw_old*q2_vw_old - 2*q1_vw_old*q3_vw_old)*(2*py_ci_old*q1_wi_old - 2*px_ci_old*q2_wi_old + 2*pz_ci_old*q0_wi_old) - (2*q0_vw_old*q1_vw_old + 2*q2_vw_old*q3_vw_old)*(2*px_ci_old*q1_wi_old + 2*py_ci_old*q2_wi_old + 2*pz_ci_old*q3_wi_old) + (2*px_ci_old*q0_wi_old - 2*py_ci_old*q3_wi_old + 2*pz_ci_old*q2_wi_old)*(q0_vw_old^2 - q1_vw_old^2 - q2_vw_old^2 + q3_vw_old^2)), -lamda_old*((2*q0_vw_old*q2_vw_old - 2*q1_vw_old*q3_vw_old)*(2*px_ci_old*q3_wi_old + 2*py_ci_old*q0_wi_old - 2*pz_ci_old*q1_wi_old) + (2*q0_vw_old*q1_vw_old + 2*q2_vw_old*q3_vw_old)*(2*px_ci_old*q0_wi_old - 2*py_ci_old*q3_wi_old + 2*pz_ci_old*q2_wi_old) + (2*px_ci_old*q1_wi_old + 2*py_ci_old*q2_wi_old + 2*pz_ci_old*q3_wi_old)*(q0_vw_old^2 - q1_vw_old^2 - q2_vw_old^2 + q3_vw_old^2))]];

% pHk(p_iw) = delta(z_p_relative) / delta(p_iw_k)
H_z_p_relative_piw_old = [[ -lamda_old*(q0_vw_old^2 + q1_vw_old^2 - q2_vw_old^2 - q3_vw_old^2),          lamda_old*(2*q0_vw_old*q3_vw_old - 2*q1_vw_old*q2_vw_old),         -lamda_old*(2*q0_vw_old*q2_vw_old + 2*q1_vw_old*q3_vw_old)]
                          [         -lamda_old*(2*q0_vw_old*q3_vw_old + 2*q1_vw_old*q2_vw_old), -lamda_old*(q0_vw_old^2 - q1_vw_old^2 + q2_vw_old^2 - q3_vw_old^2),          lamda_old*(2*q0_vw_old*q1_vw_old - 2*q2_vw_old*q3_vw_old)]
                          [          lamda_old*(2*q0_vw_old*q2_vw_old - 2*q1_vw_old*q3_vw_old),         -lamda_old*(2*q0_vw_old*q1_vw_old + 2*q2_vw_old*q3_vw_old), -lamda_old*(q0_vw_old^2 - q1_vw_old^2 - q2_vw_old^2 + q3_vw_old^2)]];

% pHk(lamda) = delta(z_p_relative) / delta(lamda_k)
if (param.fusion.camFixScale == 0)
    H_z_p_relative_lamda_old = [ (2*q0_vw_old*q3_vw_old - 2*q1_vw_old*q2_vw_old)*(py_iw_old - py_vw_old + py_ci_old*(q0_wi_old^2 - q1_wi_old^2 + q2_wi_old^2 - q3_wi_old^2) + px_ci_old*(2*q0_wi_old*q3_wi_old + 2*q1_wi_old*q2_wi_old) - pz_ci_old*(2*q0_wi_old*q1_wi_old - 2*q2_wi_old*q3_wi_old)) - (2*q0_vw_old*q2_vw_old + 2*q1_vw_old*q3_vw_old)*(pz_iw_old - pz_vw_old + pz_ci_old*(q0_wi_old^2 - q1_wi_old^2 - q2_wi_old^2 + q3_wi_old^2) - px_ci_old*(2*q0_wi_old*q2_wi_old - 2*q1_wi_old*q3_wi_old) + py_ci_old*(2*q0_wi_old*q1_wi_old + 2*q2_wi_old*q3_wi_old)) - (q0_vw_old^2 + q1_vw_old^2 - q2_vw_old^2 - q3_vw_old^2)*(px_iw_old - px_vw_old + px_ci_old*(q0_wi_old^2 + q1_wi_old^2 - q2_wi_old^2 - q3_wi_old^2) - py_ci_old*(2*q0_wi_old*q3_wi_old - 2*q1_wi_old*q2_wi_old) + pz_ci_old*(2*q0_wi_old*q2_wi_old + 2*q1_wi_old*q3_wi_old))
                                 (2*q0_vw_old*q1_vw_old - 2*q2_vw_old*q3_vw_old)*(pz_iw_old - pz_vw_old + pz_ci_old*(q0_wi_old^2 - q1_wi_old^2 - q2_wi_old^2 + q3_wi_old^2) - px_ci_old*(2*q0_wi_old*q2_wi_old - 2*q1_wi_old*q3_wi_old) + py_ci_old*(2*q0_wi_old*q1_wi_old + 2*q2_wi_old*q3_wi_old)) - (2*q0_vw_old*q3_vw_old + 2*q1_vw_old*q2_vw_old)*(px_iw_old - px_vw_old + px_ci_old*(q0_wi_old^2 + q1_wi_old^2 - q2_wi_old^2 - q3_wi_old^2) - py_ci_old*(2*q0_wi_old*q3_wi_old - 2*q1_wi_old*q2_wi_old) + pz_ci_old*(2*q0_wi_old*q2_wi_old + 2*q1_wi_old*q3_wi_old)) - (q0_vw_old^2 - q1_vw_old^2 + q2_vw_old^2 - q3_vw_old^2)*(py_iw_old - py_vw_old + py_ci_old*(q0_wi_old^2 - q1_wi_old^2 + q2_wi_old^2 - q3_wi_old^2) + px_ci_old*(2*q0_wi_old*q3_wi_old + 2*q1_wi_old*q2_wi_old) - pz_ci_old*(2*q0_wi_old*q1_wi_old - 2*q2_wi_old*q3_wi_old))
                                 (2*q0_vw_old*q2_vw_old - 2*q1_vw_old*q3_vw_old)*(px_iw_old - px_vw_old + px_ci_old*(q0_wi_old^2 + q1_wi_old^2 - q2_wi_old^2 - q3_wi_old^2) - py_ci_old*(2*q0_wi_old*q3_wi_old - 2*q1_wi_old*q2_wi_old) + pz_ci_old*(2*q0_wi_old*q2_wi_old + 2*q1_wi_old*q3_wi_old)) - (2*q0_vw_old*q1_vw_old + 2*q2_vw_old*q3_vw_old)*(py_iw_old - py_vw_old + py_ci_old*(q0_wi_old^2 - q1_wi_old^2 + q2_wi_old^2 - q3_wi_old^2) + px_ci_old*(2*q0_wi_old*q3_wi_old + 2*q1_wi_old*q2_wi_old) - pz_ci_old*(2*q0_wi_old*q1_wi_old - 2*q2_wi_old*q3_wi_old)) - (q0_vw_old^2 - q1_vw_old^2 - q2_vw_old^2 + q3_vw_old^2)*(pz_iw_old - pz_vw_old + pz_ci_old*(q0_wi_old^2 - q1_wi_old^2 - q2_wi_old^2 + q3_wi_old^2) - px_ci_old*(2*q0_wi_old*q2_wi_old - 2*q1_wi_old*q3_wi_old) + py_ci_old*(2*q0_wi_old*q1_wi_old + 2*q2_wi_old*q3_wi_old))];

elseif (param.fusion.camFixScale == 1)
    H_z_p_relative_lamda_old = zeros(3,1);
                
end

% pHk(p_ci) = delta(z_p_relative) / delta(p_ci_k)
if (param.fusion.camFixPosci == 0)
    H_z_p_relative_pci_old = [[  lamda_old*((2*q0_wi_old*q2_wi_old - 2*q1_wi_old*q3_wi_old)*(2*q0_vw_old*q2_vw_old + 2*q1_vw_old*q3_vw_old) - (q0_wi_old^2 + q1_wi_old^2 - q2_wi_old^2 - q3_wi_old^2)*(q0_vw_old^2 + q1_vw_old^2 - q2_vw_old^2 - q3_vw_old^2) + (2*q0_wi_old*q3_wi_old + 2*q1_wi_old*q2_wi_old)*(2*q0_vw_old*q3_vw_old - 2*q1_vw_old*q2_vw_old)),  lamda_old*((2*q0_wi_old*q3_wi_old - 2*q1_wi_old*q2_wi_old)*(q0_vw_old^2 + q1_vw_old^2 - q2_vw_old^2 - q3_vw_old^2) - (2*q0_wi_old*q1_wi_old + 2*q2_wi_old*q3_wi_old)*(2*q0_vw_old*q2_vw_old + 2*q1_vw_old*q3_vw_old) + (2*q0_vw_old*q3_vw_old - 2*q1_vw_old*q2_vw_old)*(q0_wi_old^2 - q1_wi_old^2 + q2_wi_old^2 - q3_wi_old^2)), -lamda_old*((2*q0_wi_old*q1_wi_old - 2*q2_wi_old*q3_wi_old)*(2*q0_vw_old*q3_vw_old - 2*q1_vw_old*q2_vw_old) + (2*q0_wi_old*q2_wi_old + 2*q1_wi_old*q3_wi_old)*(q0_vw_old^2 + q1_vw_old^2 - q2_vw_old^2 - q3_vw_old^2) + (2*q0_vw_old*q2_vw_old + 2*q1_vw_old*q3_vw_old)*(q0_wi_old^2 - q1_wi_old^2 - q2_wi_old^2 + q3_wi_old^2))]
                              [ -lamda_old*((2*q0_wi_old*q2_wi_old - 2*q1_wi_old*q3_wi_old)*(2*q0_vw_old*q1_vw_old - 2*q2_vw_old*q3_vw_old) + (2*q0_wi_old*q3_wi_old + 2*q1_wi_old*q2_wi_old)*(q0_vw_old^2 - q1_vw_old^2 + q2_vw_old^2 - q3_vw_old^2) + (2*q0_vw_old*q3_vw_old + 2*q1_vw_old*q2_vw_old)*(q0_wi_old^2 + q1_wi_old^2 - q2_wi_old^2 - q3_wi_old^2)),  lamda_old*((2*q0_wi_old*q1_wi_old + 2*q2_wi_old*q3_wi_old)*(2*q0_vw_old*q1_vw_old - 2*q2_vw_old*q3_vw_old) - (q0_wi_old^2 - q1_wi_old^2 + q2_wi_old^2 - q3_wi_old^2)*(q0_vw_old^2 - q1_vw_old^2 + q2_vw_old^2 - q3_vw_old^2) + (2*q0_wi_old*q3_wi_old - 2*q1_wi_old*q2_wi_old)*(2*q0_vw_old*q3_vw_old + 2*q1_vw_old*q2_vw_old)),  lamda_old*((2*q0_wi_old*q1_wi_old - 2*q2_wi_old*q3_wi_old)*(q0_vw_old^2 - q1_vw_old^2 + q2_vw_old^2 - q3_vw_old^2) - (2*q0_wi_old*q2_wi_old + 2*q1_wi_old*q3_wi_old)*(2*q0_vw_old*q3_vw_old + 2*q1_vw_old*q2_vw_old) + (2*q0_vw_old*q1_vw_old - 2*q2_vw_old*q3_vw_old)*(q0_wi_old^2 - q1_wi_old^2 - q2_wi_old^2 + q3_wi_old^2))]
                              [  lamda_old*((2*q0_wi_old*q2_wi_old - 2*q1_wi_old*q3_wi_old)*(q0_vw_old^2 - q1_vw_old^2 - q2_vw_old^2 + q3_vw_old^2) - (2*q0_wi_old*q3_wi_old + 2*q1_wi_old*q2_wi_old)*(2*q0_vw_old*q1_vw_old + 2*q2_vw_old*q3_vw_old) + (2*q0_vw_old*q2_vw_old - 2*q1_vw_old*q3_vw_old)*(q0_wi_old^2 + q1_wi_old^2 - q2_wi_old^2 - q3_wi_old^2)), -lamda_old*((2*q0_wi_old*q3_wi_old - 2*q1_wi_old*q2_wi_old)*(2*q0_vw_old*q2_vw_old - 2*q1_vw_old*q3_vw_old) + (2*q0_wi_old*q1_wi_old + 2*q2_wi_old*q3_wi_old)*(q0_vw_old^2 - q1_vw_old^2 - q2_vw_old^2 + q3_vw_old^2) + (2*q0_vw_old*q1_vw_old + 2*q2_vw_old*q3_vw_old)*(q0_wi_old^2 - q1_wi_old^2 + q2_wi_old^2 - q3_wi_old^2)),  lamda_old*((2*q0_wi_old*q1_wi_old - 2*q2_wi_old*q3_wi_old)*(2*q0_vw_old*q1_vw_old + 2*q2_vw_old*q3_vw_old) - (q0_wi_old^2 - q1_wi_old^2 - q2_wi_old^2 + q3_wi_old^2)*(q0_vw_old^2 - q1_vw_old^2 - q2_vw_old^2 + q3_vw_old^2) + (2*q0_wi_old*q2_wi_old + 2*q1_wi_old*q3_wi_old)*(2*q0_vw_old*q2_vw_old - 2*q1_vw_old*q3_vw_old))]];

elseif (param.fusion.camFixPosci == 1)
    H_z_p_relative_pci_old = zeros(3,3);
                
end

% pHk(p_vw) = delta(z_p_relative) / delta(p_vw_k)
if (param.fusion.camFixPosvw == 0)
    H_z_p_relative_pvw_old = [[ lamda_old*(q0_vw_old^2 + q1_vw_old^2 - q2_vw_old^2 - q3_vw_old^2),        -lamda_old*(2*q0_vw_old*q3_vw_old - 2*q1_vw_old*q2_vw_old),         lamda_old*(2*q0_vw_old*q2_vw_old + 2*q1_vw_old*q3_vw_old)]
                              [         lamda_old*(2*q0_vw_old*q3_vw_old + 2*q1_vw_old*q2_vw_old), lamda_old*(q0_vw_old^2 - q1_vw_old^2 + q2_vw_old^2 - q3_vw_old^2),        -lamda_old*(2*q0_vw_old*q1_vw_old - 2*q2_vw_old*q3_vw_old)]
                              [        -lamda_old*(2*q0_vw_old*q2_vw_old - 2*q1_vw_old*q3_vw_old),         lamda_old*(2*q0_vw_old*q1_vw_old + 2*q2_vw_old*q3_vw_old), lamda_old*(q0_vw_old^2 - q1_vw_old^2 - q2_vw_old^2 + q3_vw_old^2)]];

elseif (param.fusion.camFixPosvw == 1)
    H_z_p_relative_pvw_old = zeros(3,3);
                
end

% pHk(q_vw) = delta(z_p_relative) / delta(q_vw_k)
if (param.fusion.camFixQuatvw == 0)
    H_z_p_relative_qvw_old = [[ -lamda_old*(2*q0_vw_old*(px_iw_old - px_vw_old + px_ci_old*(q0_wi_old^2 + q1_wi_old^2 - q2_wi_old^2 - q3_wi_old^2) - py_ci_old*(2*q0_wi_old*q3_wi_old - 2*q1_wi_old*q2_wi_old) + pz_ci_old*(2*q0_wi_old*q2_wi_old + 2*q1_wi_old*q3_wi_old)) - 2*q3_vw_old*(py_iw_old - py_vw_old + py_ci_old*(q0_wi_old^2 - q1_wi_old^2 + q2_wi_old^2 - q3_wi_old^2) + px_ci_old*(2*q0_wi_old*q3_wi_old + 2*q1_wi_old*q2_wi_old) - pz_ci_old*(2*q0_wi_old*q1_wi_old - 2*q2_wi_old*q3_wi_old)) + 2*q2_vw_old*(pz_iw_old - pz_vw_old + pz_ci_old*(q0_wi_old^2 - q1_wi_old^2 - q2_wi_old^2 + q3_wi_old^2) - px_ci_old*(2*q0_wi_old*q2_wi_old - 2*q1_wi_old*q3_wi_old) + py_ci_old*(2*q0_wi_old*q1_wi_old + 2*q2_wi_old*q3_wi_old))), -lamda_old*(2*q1_vw_old*(px_iw_old - px_vw_old + px_ci_old*(q0_wi_old^2 + q1_wi_old^2 - q2_wi_old^2 - q3_wi_old^2) - py_ci_old*(2*q0_wi_old*q3_wi_old - 2*q1_wi_old*q2_wi_old) + pz_ci_old*(2*q0_wi_old*q2_wi_old + 2*q1_wi_old*q3_wi_old)) + 2*q2_vw_old*(py_iw_old - py_vw_old + py_ci_old*(q0_wi_old^2 - q1_wi_old^2 + q2_wi_old^2 - q3_wi_old^2) + px_ci_old*(2*q0_wi_old*q3_wi_old + 2*q1_wi_old*q2_wi_old) - pz_ci_old*(2*q0_wi_old*q1_wi_old - 2*q2_wi_old*q3_wi_old)) + 2*q3_vw_old*(pz_iw_old - pz_vw_old + pz_ci_old*(q0_wi_old^2 - q1_wi_old^2 - q2_wi_old^2 + q3_wi_old^2) - px_ci_old*(2*q0_wi_old*q2_wi_old - 2*q1_wi_old*q3_wi_old) + py_ci_old*(2*q0_wi_old*q1_wi_old + 2*q2_wi_old*q3_wi_old))), -lamda_old*(2*q1_vw_old*(py_iw_old - py_vw_old + py_ci_old*(q0_wi_old^2 - q1_wi_old^2 + q2_wi_old^2 - q3_wi_old^2) + px_ci_old*(2*q0_wi_old*q3_wi_old + 2*q1_wi_old*q2_wi_old) - pz_ci_old*(2*q0_wi_old*q1_wi_old - 2*q2_wi_old*q3_wi_old)) - 2*q2_vw_old*(px_iw_old - px_vw_old + px_ci_old*(q0_wi_old^2 + q1_wi_old^2 - q2_wi_old^2 - q3_wi_old^2) - py_ci_old*(2*q0_wi_old*q3_wi_old - 2*q1_wi_old*q2_wi_old) + pz_ci_old*(2*q0_wi_old*q2_wi_old + 2*q1_wi_old*q3_wi_old)) + 2*q0_vw_old*(pz_iw_old - pz_vw_old + pz_ci_old*(q0_wi_old^2 - q1_wi_old^2 - q2_wi_old^2 + q3_wi_old^2) - px_ci_old*(2*q0_wi_old*q2_wi_old - 2*q1_wi_old*q3_wi_old) + py_ci_old*(2*q0_wi_old*q1_wi_old + 2*q2_wi_old*q3_wi_old))),  lamda_old*(2*q3_vw_old*(px_iw_old - px_vw_old + px_ci_old*(q0_wi_old^2 + q1_wi_old^2 - q2_wi_old^2 - q3_wi_old^2) - py_ci_old*(2*q0_wi_old*q3_wi_old - 2*q1_wi_old*q2_wi_old) + pz_ci_old*(2*q0_wi_old*q2_wi_old + 2*q1_wi_old*q3_wi_old)) + 2*q0_vw_old*(py_iw_old - py_vw_old + py_ci_old*(q0_wi_old^2 - q1_wi_old^2 + q2_wi_old^2 - q3_wi_old^2) + px_ci_old*(2*q0_wi_old*q3_wi_old + 2*q1_wi_old*q2_wi_old) - pz_ci_old*(2*q0_wi_old*q1_wi_old - 2*q2_wi_old*q3_wi_old)) - 2*q1_vw_old*(pz_iw_old - pz_vw_old + pz_ci_old*(q0_wi_old^2 - q1_wi_old^2 - q2_wi_old^2 + q3_wi_old^2) - px_ci_old*(2*q0_wi_old*q2_wi_old - 2*q1_wi_old*q3_wi_old) + py_ci_old*(2*q0_wi_old*q1_wi_old + 2*q2_wi_old*q3_wi_old)))]
                              [ -lamda_old*(2*q3_vw_old*(px_iw_old - px_vw_old + px_ci_old*(q0_wi_old^2 + q1_wi_old^2 - q2_wi_old^2 - q3_wi_old^2) - py_ci_old*(2*q0_wi_old*q3_wi_old - 2*q1_wi_old*q2_wi_old) + pz_ci_old*(2*q0_wi_old*q2_wi_old + 2*q1_wi_old*q3_wi_old)) + 2*q0_vw_old*(py_iw_old - py_vw_old + py_ci_old*(q0_wi_old^2 - q1_wi_old^2 + q2_wi_old^2 - q3_wi_old^2) + px_ci_old*(2*q0_wi_old*q3_wi_old + 2*q1_wi_old*q2_wi_old) - pz_ci_old*(2*q0_wi_old*q1_wi_old - 2*q2_wi_old*q3_wi_old)) - 2*q1_vw_old*(pz_iw_old - pz_vw_old + pz_ci_old*(q0_wi_old^2 - q1_wi_old^2 - q2_wi_old^2 + q3_wi_old^2) - px_ci_old*(2*q0_wi_old*q2_wi_old - 2*q1_wi_old*q3_wi_old) + py_ci_old*(2*q0_wi_old*q1_wi_old + 2*q2_wi_old*q3_wi_old))),  lamda_old*(2*q1_vw_old*(py_iw_old - py_vw_old + py_ci_old*(q0_wi_old^2 - q1_wi_old^2 + q2_wi_old^2 - q3_wi_old^2) + px_ci_old*(2*q0_wi_old*q3_wi_old + 2*q1_wi_old*q2_wi_old) - pz_ci_old*(2*q0_wi_old*q1_wi_old - 2*q2_wi_old*q3_wi_old)) - 2*q2_vw_old*(px_iw_old - px_vw_old + px_ci_old*(q0_wi_old^2 + q1_wi_old^2 - q2_wi_old^2 - q3_wi_old^2) - py_ci_old*(2*q0_wi_old*q3_wi_old - 2*q1_wi_old*q2_wi_old) + pz_ci_old*(2*q0_wi_old*q2_wi_old + 2*q1_wi_old*q3_wi_old)) + 2*q0_vw_old*(pz_iw_old - pz_vw_old + pz_ci_old*(q0_wi_old^2 - q1_wi_old^2 - q2_wi_old^2 + q3_wi_old^2) - px_ci_old*(2*q0_wi_old*q2_wi_old - 2*q1_wi_old*q3_wi_old) + py_ci_old*(2*q0_wi_old*q1_wi_old + 2*q2_wi_old*q3_wi_old))), -lamda_old*(2*q1_vw_old*(px_iw_old - px_vw_old + px_ci_old*(q0_wi_old^2 + q1_wi_old^2 - q2_wi_old^2 - q3_wi_old^2) - py_ci_old*(2*q0_wi_old*q3_wi_old - 2*q1_wi_old*q2_wi_old) + pz_ci_old*(2*q0_wi_old*q2_wi_old + 2*q1_wi_old*q3_wi_old)) + 2*q2_vw_old*(py_iw_old - py_vw_old + py_ci_old*(q0_wi_old^2 - q1_wi_old^2 + q2_wi_old^2 - q3_wi_old^2) + px_ci_old*(2*q0_wi_old*q3_wi_old + 2*q1_wi_old*q2_wi_old) - pz_ci_old*(2*q0_wi_old*q1_wi_old - 2*q2_wi_old*q3_wi_old)) + 2*q3_vw_old*(pz_iw_old - pz_vw_old + pz_ci_old*(q0_wi_old^2 - q1_wi_old^2 - q2_wi_old^2 + q3_wi_old^2) - px_ci_old*(2*q0_wi_old*q2_wi_old - 2*q1_wi_old*q3_wi_old) + py_ci_old*(2*q0_wi_old*q1_wi_old + 2*q2_wi_old*q3_wi_old))), -lamda_old*(2*q0_vw_old*(px_iw_old - px_vw_old + px_ci_old*(q0_wi_old^2 + q1_wi_old^2 - q2_wi_old^2 - q3_wi_old^2) - py_ci_old*(2*q0_wi_old*q3_wi_old - 2*q1_wi_old*q2_wi_old) + pz_ci_old*(2*q0_wi_old*q2_wi_old + 2*q1_wi_old*q3_wi_old)) - 2*q3_vw_old*(py_iw_old - py_vw_old + py_ci_old*(q0_wi_old^2 - q1_wi_old^2 + q2_wi_old^2 - q3_wi_old^2) + px_ci_old*(2*q0_wi_old*q3_wi_old + 2*q1_wi_old*q2_wi_old) - pz_ci_old*(2*q0_wi_old*q1_wi_old - 2*q2_wi_old*q3_wi_old)) + 2*q2_vw_old*(pz_iw_old - pz_vw_old + pz_ci_old*(q0_wi_old^2 - q1_wi_old^2 - q2_wi_old^2 + q3_wi_old^2) - px_ci_old*(2*q0_wi_old*q2_wi_old - 2*q1_wi_old*q3_wi_old) + py_ci_old*(2*q0_wi_old*q1_wi_old + 2*q2_wi_old*q3_wi_old)))]
                              [ -lamda_old*(2*q1_vw_old*(py_iw_old - py_vw_old + py_ci_old*(q0_wi_old^2 - q1_wi_old^2 + q2_wi_old^2 - q3_wi_old^2) + px_ci_old*(2*q0_wi_old*q3_wi_old + 2*q1_wi_old*q2_wi_old) - pz_ci_old*(2*q0_wi_old*q1_wi_old - 2*q2_wi_old*q3_wi_old)) - 2*q2_vw_old*(px_iw_old - px_vw_old + px_ci_old*(q0_wi_old^2 + q1_wi_old^2 - q2_wi_old^2 - q3_wi_old^2) - py_ci_old*(2*q0_wi_old*q3_wi_old - 2*q1_wi_old*q2_wi_old) + pz_ci_old*(2*q0_wi_old*q2_wi_old + 2*q1_wi_old*q3_wi_old)) + 2*q0_vw_old*(pz_iw_old - pz_vw_old + pz_ci_old*(q0_wi_old^2 - q1_wi_old^2 - q2_wi_old^2 + q3_wi_old^2) - px_ci_old*(2*q0_wi_old*q2_wi_old - 2*q1_wi_old*q3_wi_old) + py_ci_old*(2*q0_wi_old*q1_wi_old + 2*q2_wi_old*q3_wi_old))), -lamda_old*(2*q3_vw_old*(px_iw_old - px_vw_old + px_ci_old*(q0_wi_old^2 + q1_wi_old^2 - q2_wi_old^2 - q3_wi_old^2) - py_ci_old*(2*q0_wi_old*q3_wi_old - 2*q1_wi_old*q2_wi_old) + pz_ci_old*(2*q0_wi_old*q2_wi_old + 2*q1_wi_old*q3_wi_old)) + 2*q0_vw_old*(py_iw_old - py_vw_old + py_ci_old*(q0_wi_old^2 - q1_wi_old^2 + q2_wi_old^2 - q3_wi_old^2) + px_ci_old*(2*q0_wi_old*q3_wi_old + 2*q1_wi_old*q2_wi_old) - pz_ci_old*(2*q0_wi_old*q1_wi_old - 2*q2_wi_old*q3_wi_old)) - 2*q1_vw_old*(pz_iw_old - pz_vw_old + pz_ci_old*(q0_wi_old^2 - q1_wi_old^2 - q2_wi_old^2 + q3_wi_old^2) - px_ci_old*(2*q0_wi_old*q2_wi_old - 2*q1_wi_old*q3_wi_old) + py_ci_old*(2*q0_wi_old*q1_wi_old + 2*q2_wi_old*q3_wi_old))),  lamda_old*(2*q0_vw_old*(px_iw_old - px_vw_old + px_ci_old*(q0_wi_old^2 + q1_wi_old^2 - q2_wi_old^2 - q3_wi_old^2) - py_ci_old*(2*q0_wi_old*q3_wi_old - 2*q1_wi_old*q2_wi_old) + pz_ci_old*(2*q0_wi_old*q2_wi_old + 2*q1_wi_old*q3_wi_old)) - 2*q3_vw_old*(py_iw_old - py_vw_old + py_ci_old*(q0_wi_old^2 - q1_wi_old^2 + q2_wi_old^2 - q3_wi_old^2) + px_ci_old*(2*q0_wi_old*q3_wi_old + 2*q1_wi_old*q2_wi_old) - pz_ci_old*(2*q0_wi_old*q1_wi_old - 2*q2_wi_old*q3_wi_old)) + 2*q2_vw_old*(pz_iw_old - pz_vw_old + pz_ci_old*(q0_wi_old^2 - q1_wi_old^2 - q2_wi_old^2 + q3_wi_old^2) - px_ci_old*(2*q0_wi_old*q2_wi_old - 2*q1_wi_old*q3_wi_old) + py_ci_old*(2*q0_wi_old*q1_wi_old + 2*q2_wi_old*q3_wi_old))), -lamda_old*(2*q1_vw_old*(px_iw_old - px_vw_old + px_ci_old*(q0_wi_old^2 + q1_wi_old^2 - q2_wi_old^2 - q3_wi_old^2) - py_ci_old*(2*q0_wi_old*q3_wi_old - 2*q1_wi_old*q2_wi_old) + pz_ci_old*(2*q0_wi_old*q2_wi_old + 2*q1_wi_old*q3_wi_old)) + 2*q2_vw_old*(py_iw_old - py_vw_old + py_ci_old*(q0_wi_old^2 - q1_wi_old^2 + q2_wi_old^2 - q3_wi_old^2) + px_ci_old*(2*q0_wi_old*q3_wi_old + 2*q1_wi_old*q2_wi_old) - pz_ci_old*(2*q0_wi_old*q1_wi_old - 2*q2_wi_old*q3_wi_old)) + 2*q3_vw_old*(pz_iw_old - pz_vw_old + pz_ci_old*(q0_wi_old^2 - q1_wi_old^2 - q2_wi_old^2 + q3_wi_old^2) - px_ci_old*(2*q0_wi_old*q2_wi_old - 2*q1_wi_old*q3_wi_old) + py_ci_old*(2*q0_wi_old*q1_wi_old + 2*q2_wi_old*q3_wi_old)))]];

elseif (param.fusion.camFixQuatvw == 1)
    H_z_p_relative_qvw_old = zeros(3,4);
                
end


%% pH(k+m)

% pH(k+m)(q_wi) = delta(z_p_relative) / delta(q_wi_k+m)
H_z_p_relative_qwi_new = [[ lamda_new*((2*q0_vw_new*q2_vw_new + 2*q1_vw_new*q3_vw_new)*(2*py_ci_new*q1_wi_new - 2*px_ci_new*q2_wi_new + 2*pz_ci_new*q0_wi_new) - (2*q0_vw_new*q3_vw_new - 2*q1_vw_new*q2_vw_new)*(2*px_ci_new*q3_wi_new + 2*py_ci_new*q0_wi_new - 2*pz_ci_new*q1_wi_new) + (2*px_ci_new*q0_wi_new - 2*py_ci_new*q3_wi_new + 2*pz_ci_new*q2_wi_new)*(q0_vw_new^2 + q1_vw_new^2 - q2_vw_new^2 - q3_vw_new^2)),  lamda_new*((2*q0_vw_new*q3_vw_new - 2*q1_vw_new*q2_vw_new)*(2*py_ci_new*q1_wi_new - 2*px_ci_new*q2_wi_new + 2*pz_ci_new*q0_wi_new) + (2*q0_vw_new*q2_vw_new + 2*q1_vw_new*q3_vw_new)*(2*px_ci_new*q3_wi_new + 2*py_ci_new*q0_wi_new - 2*pz_ci_new*q1_wi_new) + (2*px_ci_new*q1_wi_new + 2*py_ci_new*q2_wi_new + 2*pz_ci_new*q3_wi_new)*(q0_vw_new^2 + q1_vw_new^2 - q2_vw_new^2 - q3_vw_new^2)), -lamda_new*((2*q0_vw_new*q2_vw_new + 2*q1_vw_new*q3_vw_new)*(2*px_ci_new*q0_wi_new - 2*py_ci_new*q3_wi_new + 2*pz_ci_new*q2_wi_new) + (2*q0_vw_new*q3_vw_new - 2*q1_vw_new*q2_vw_new)*(2*px_ci_new*q1_wi_new + 2*py_ci_new*q2_wi_new + 2*pz_ci_new*q3_wi_new) - (2*py_ci_new*q1_wi_new - 2*px_ci_new*q2_wi_new + 2*pz_ci_new*q0_wi_new)*(q0_vw_new^2 + q1_vw_new^2 - q2_vw_new^2 - q3_vw_new^2)), -lamda_new*((2*q0_vw_new*q3_vw_new - 2*q1_vw_new*q2_vw_new)*(2*px_ci_new*q0_wi_new - 2*py_ci_new*q3_wi_new + 2*pz_ci_new*q2_wi_new) - (2*q0_vw_new*q2_vw_new + 2*q1_vw_new*q3_vw_new)*(2*px_ci_new*q1_wi_new + 2*py_ci_new*q2_wi_new + 2*pz_ci_new*q3_wi_new) + (2*px_ci_new*q3_wi_new + 2*py_ci_new*q0_wi_new - 2*pz_ci_new*q1_wi_new)*(q0_vw_new^2 + q1_vw_new^2 - q2_vw_new^2 - q3_vw_new^2))]
                          [ lamda_new*((2*q0_vw_new*q3_vw_new + 2*q1_vw_new*q2_vw_new)*(2*px_ci_new*q0_wi_new - 2*py_ci_new*q3_wi_new + 2*pz_ci_new*q2_wi_new) - (2*q0_vw_new*q1_vw_new - 2*q2_vw_new*q3_vw_new)*(2*py_ci_new*q1_wi_new - 2*px_ci_new*q2_wi_new + 2*pz_ci_new*q0_wi_new) + (2*px_ci_new*q3_wi_new + 2*py_ci_new*q0_wi_new - 2*pz_ci_new*q1_wi_new)*(q0_vw_new^2 - q1_vw_new^2 + q2_vw_new^2 - q3_vw_new^2)), -lamda_new*((2*q0_vw_new*q1_vw_new - 2*q2_vw_new*q3_vw_new)*(2*px_ci_new*q3_wi_new + 2*py_ci_new*q0_wi_new - 2*pz_ci_new*q1_wi_new) - (2*q0_vw_new*q3_vw_new + 2*q1_vw_new*q2_vw_new)*(2*px_ci_new*q1_wi_new + 2*py_ci_new*q2_wi_new + 2*pz_ci_new*q3_wi_new) + (2*py_ci_new*q1_wi_new - 2*px_ci_new*q2_wi_new + 2*pz_ci_new*q0_wi_new)*(q0_vw_new^2 - q1_vw_new^2 + q2_vw_new^2 - q3_vw_new^2)),  lamda_new*((2*q0_vw_new*q3_vw_new + 2*q1_vw_new*q2_vw_new)*(2*py_ci_new*q1_wi_new - 2*px_ci_new*q2_wi_new + 2*pz_ci_new*q0_wi_new) + (2*q0_vw_new*q1_vw_new - 2*q2_vw_new*q3_vw_new)*(2*px_ci_new*q0_wi_new - 2*py_ci_new*q3_wi_new + 2*pz_ci_new*q2_wi_new) + (2*px_ci_new*q1_wi_new + 2*py_ci_new*q2_wi_new + 2*pz_ci_new*q3_wi_new)*(q0_vw_new^2 - q1_vw_new^2 + q2_vw_new^2 - q3_vw_new^2)), -lamda_new*((2*q0_vw_new*q3_vw_new + 2*q1_vw_new*q2_vw_new)*(2*px_ci_new*q3_wi_new + 2*py_ci_new*q0_wi_new - 2*pz_ci_new*q1_wi_new) + (2*q0_vw_new*q1_vw_new - 2*q2_vw_new*q3_vw_new)*(2*px_ci_new*q1_wi_new + 2*py_ci_new*q2_wi_new + 2*pz_ci_new*q3_wi_new) - (2*px_ci_new*q0_wi_new - 2*py_ci_new*q3_wi_new + 2*pz_ci_new*q2_wi_new)*(q0_vw_new^2 - q1_vw_new^2 + q2_vw_new^2 - q3_vw_new^2))]
                          [ lamda_new*((2*q0_vw_new*q1_vw_new + 2*q2_vw_new*q3_vw_new)*(2*px_ci_new*q3_wi_new + 2*py_ci_new*q0_wi_new - 2*pz_ci_new*q1_wi_new) - (2*q0_vw_new*q2_vw_new - 2*q1_vw_new*q3_vw_new)*(2*px_ci_new*q0_wi_new - 2*py_ci_new*q3_wi_new + 2*pz_ci_new*q2_wi_new) + (2*py_ci_new*q1_wi_new - 2*px_ci_new*q2_wi_new + 2*pz_ci_new*q0_wi_new)*(q0_vw_new^2 - q1_vw_new^2 - q2_vw_new^2 + q3_vw_new^2)), -lamda_new*((2*q0_vw_new*q1_vw_new + 2*q2_vw_new*q3_vw_new)*(2*py_ci_new*q1_wi_new - 2*px_ci_new*q2_wi_new + 2*pz_ci_new*q0_wi_new) + (2*q0_vw_new*q2_vw_new - 2*q1_vw_new*q3_vw_new)*(2*px_ci_new*q1_wi_new + 2*py_ci_new*q2_wi_new + 2*pz_ci_new*q3_wi_new) - (2*px_ci_new*q3_wi_new + 2*py_ci_new*q0_wi_new - 2*pz_ci_new*q1_wi_new)*(q0_vw_new^2 - q1_vw_new^2 - q2_vw_new^2 + q3_vw_new^2)), -lamda_new*((2*q0_vw_new*q2_vw_new - 2*q1_vw_new*q3_vw_new)*(2*py_ci_new*q1_wi_new - 2*px_ci_new*q2_wi_new + 2*pz_ci_new*q0_wi_new) - (2*q0_vw_new*q1_vw_new + 2*q2_vw_new*q3_vw_new)*(2*px_ci_new*q1_wi_new + 2*py_ci_new*q2_wi_new + 2*pz_ci_new*q3_wi_new) + (2*px_ci_new*q0_wi_new - 2*py_ci_new*q3_wi_new + 2*pz_ci_new*q2_wi_new)*(q0_vw_new^2 - q1_vw_new^2 - q2_vw_new^2 + q3_vw_new^2)),  lamda_new*((2*q0_vw_new*q2_vw_new - 2*q1_vw_new*q3_vw_new)*(2*px_ci_new*q3_wi_new + 2*py_ci_new*q0_wi_new - 2*pz_ci_new*q1_wi_new) + (2*q0_vw_new*q1_vw_new + 2*q2_vw_new*q3_vw_new)*(2*px_ci_new*q0_wi_new - 2*py_ci_new*q3_wi_new + 2*pz_ci_new*q2_wi_new) + (2*px_ci_new*q1_wi_new + 2*py_ci_new*q2_wi_new + 2*pz_ci_new*q3_wi_new)*(q0_vw_new^2 - q1_vw_new^2 - q2_vw_new^2 + q3_vw_new^2))]];

% pH(k+m)(p_iw) = delta(z_p_relative) / delta(p_iw_k+m)
H_z_p_relative_piw_new = [[ lamda_new*(q0_vw_new^2 + q1_vw_new^2 - q2_vw_new^2 - q3_vw_new^2),        -lamda_new*(2*q0_vw_new*q3_vw_new - 2*q1_vw_new*q2_vw_new),         lamda_new*(2*q0_vw_new*q2_vw_new + 2*q1_vw_new*q3_vw_new)]
                          [         lamda_new*(2*q0_vw_new*q3_vw_new + 2*q1_vw_new*q2_vw_new), lamda_new*(q0_vw_new^2 - q1_vw_new^2 + q2_vw_new^2 - q3_vw_new^2),        -lamda_new*(2*q0_vw_new*q1_vw_new - 2*q2_vw_new*q3_vw_new)]
                          [        -lamda_new*(2*q0_vw_new*q2_vw_new - 2*q1_vw_new*q3_vw_new),         lamda_new*(2*q0_vw_new*q1_vw_new + 2*q2_vw_new*q3_vw_new), lamda_new*(q0_vw_new^2 - q1_vw_new^2 - q2_vw_new^2 + q3_vw_new^2)]];

% pH(k+m)(lamda) = delta(z_p_relative) / delta(lamda_k+m)
if (param.fusion.camFixScale == 0)
    H_z_p_relative_lamda_new = [(2*q0_vw_new*q2_vw_new + 2*q1_vw_new*q3_vw_new)*(pz_iw_new - pz_vw_new + pz_ci_new*(q0_wi_new^2 - q1_wi_new^2 - q2_wi_new^2 + q3_wi_new^2) - px_ci_new*(2*q0_wi_new*q2_wi_new - 2*q1_wi_new*q3_wi_new) + py_ci_new*(2*q0_wi_new*q1_wi_new + 2*q2_wi_new*q3_wi_new)) - (2*q0_vw_new*q3_vw_new - 2*q1_vw_new*q2_vw_new)*(py_iw_new - py_vw_new + py_ci_new*(q0_wi_new^2 - q1_wi_new^2 + q2_wi_new^2 - q3_wi_new^2) + px_ci_new*(2*q0_wi_new*q3_wi_new + 2*q1_wi_new*q2_wi_new) - pz_ci_new*(2*q0_wi_new*q1_wi_new - 2*q2_wi_new*q3_wi_new)) + (q0_vw_new^2 + q1_vw_new^2 - q2_vw_new^2 - q3_vw_new^2)*(px_iw_new - px_vw_new + px_ci_new*(q0_wi_new^2 + q1_wi_new^2 - q2_wi_new^2 - q3_wi_new^2) - py_ci_new*(2*q0_wi_new*q3_wi_new - 2*q1_wi_new*q2_wi_new) + pz_ci_new*(2*q0_wi_new*q2_wi_new + 2*q1_wi_new*q3_wi_new))
                                (2*q0_vw_new*q3_vw_new + 2*q1_vw_new*q2_vw_new)*(px_iw_new - px_vw_new + px_ci_new*(q0_wi_new^2 + q1_wi_new^2 - q2_wi_new^2 - q3_wi_new^2) - py_ci_new*(2*q0_wi_new*q3_wi_new - 2*q1_wi_new*q2_wi_new) + pz_ci_new*(2*q0_wi_new*q2_wi_new + 2*q1_wi_new*q3_wi_new)) - (2*q0_vw_new*q1_vw_new - 2*q2_vw_new*q3_vw_new)*(pz_iw_new - pz_vw_new + pz_ci_new*(q0_wi_new^2 - q1_wi_new^2 - q2_wi_new^2 + q3_wi_new^2) - px_ci_new*(2*q0_wi_new*q2_wi_new - 2*q1_wi_new*q3_wi_new) + py_ci_new*(2*q0_wi_new*q1_wi_new + 2*q2_wi_new*q3_wi_new)) + (q0_vw_new^2 - q1_vw_new^2 + q2_vw_new^2 - q3_vw_new^2)*(py_iw_new - py_vw_new + py_ci_new*(q0_wi_new^2 - q1_wi_new^2 + q2_wi_new^2 - q3_wi_new^2) + px_ci_new*(2*q0_wi_new*q3_wi_new + 2*q1_wi_new*q2_wi_new) - pz_ci_new*(2*q0_wi_new*q1_wi_new - 2*q2_wi_new*q3_wi_new))
                                (2*q0_vw_new*q1_vw_new + 2*q2_vw_new*q3_vw_new)*(py_iw_new - py_vw_new + py_ci_new*(q0_wi_new^2 - q1_wi_new^2 + q2_wi_new^2 - q3_wi_new^2) + px_ci_new*(2*q0_wi_new*q3_wi_new + 2*q1_wi_new*q2_wi_new) - pz_ci_new*(2*q0_wi_new*q1_wi_new - 2*q2_wi_new*q3_wi_new)) - (2*q0_vw_new*q2_vw_new - 2*q1_vw_new*q3_vw_new)*(px_iw_new - px_vw_new + px_ci_new*(q0_wi_new^2 + q1_wi_new^2 - q2_wi_new^2 - q3_wi_new^2) - py_ci_new*(2*q0_wi_new*q3_wi_new - 2*q1_wi_new*q2_wi_new) + pz_ci_new*(2*q0_wi_new*q2_wi_new + 2*q1_wi_new*q3_wi_new)) + (q0_vw_new^2 - q1_vw_new^2 - q2_vw_new^2 + q3_vw_new^2)*(pz_iw_new - pz_vw_new + pz_ci_new*(q0_wi_new^2 - q1_wi_new^2 - q2_wi_new^2 + q3_wi_new^2) - px_ci_new*(2*q0_wi_new*q2_wi_new - 2*q1_wi_new*q3_wi_new) + py_ci_new*(2*q0_wi_new*q1_wi_new + 2*q2_wi_new*q3_wi_new))];

elseif (param.fusion.camFixScale == 1)
    H_z_p_relative_lamda_new = zeros(3,1);
                
end

% pH(k+m)(p_ci) = delta(z_p_relative) / delta(p_ci_k+m)
if (param.fusion.camFixPosci == 0)
    H_z_p_relative_pci_new = [[ -lamda_new*((2*q0_wi_new*q2_wi_new - 2*q1_wi_new*q3_wi_new)*(2*q0_vw_new*q2_vw_new + 2*q1_vw_new*q3_vw_new) - (q0_wi_new^2 + q1_wi_new^2 - q2_wi_new^2 - q3_wi_new^2)*(q0_vw_new^2 + q1_vw_new^2 - q2_vw_new^2 - q3_vw_new^2) + (2*q0_wi_new*q3_wi_new + 2*q1_wi_new*q2_wi_new)*(2*q0_vw_new*q3_vw_new - 2*q1_vw_new*q2_vw_new)), -lamda_new*((2*q0_wi_new*q3_wi_new - 2*q1_wi_new*q2_wi_new)*(q0_vw_new^2 + q1_vw_new^2 - q2_vw_new^2 - q3_vw_new^2) - (2*q0_wi_new*q1_wi_new + 2*q2_wi_new*q3_wi_new)*(2*q0_vw_new*q2_vw_new + 2*q1_vw_new*q3_vw_new) + (2*q0_vw_new*q3_vw_new - 2*q1_vw_new*q2_vw_new)*(q0_wi_new^2 - q1_wi_new^2 + q2_wi_new^2 - q3_wi_new^2)),  lamda_new*((2*q0_wi_new*q1_wi_new - 2*q2_wi_new*q3_wi_new)*(2*q0_vw_new*q3_vw_new - 2*q1_vw_new*q2_vw_new) + (2*q0_wi_new*q2_wi_new + 2*q1_wi_new*q3_wi_new)*(q0_vw_new^2 + q1_vw_new^2 - q2_vw_new^2 - q3_vw_new^2) + (2*q0_vw_new*q2_vw_new + 2*q1_vw_new*q3_vw_new)*(q0_wi_new^2 - q1_wi_new^2 - q2_wi_new^2 + q3_wi_new^2))]
                              [  lamda_new*((2*q0_wi_new*q2_wi_new - 2*q1_wi_new*q3_wi_new)*(2*q0_vw_new*q1_vw_new - 2*q2_vw_new*q3_vw_new) + (2*q0_wi_new*q3_wi_new + 2*q1_wi_new*q2_wi_new)*(q0_vw_new^2 - q1_vw_new^2 + q2_vw_new^2 - q3_vw_new^2) + (2*q0_vw_new*q3_vw_new + 2*q1_vw_new*q2_vw_new)*(q0_wi_new^2 + q1_wi_new^2 - q2_wi_new^2 - q3_wi_new^2)), -lamda_new*((2*q0_wi_new*q1_wi_new + 2*q2_wi_new*q3_wi_new)*(2*q0_vw_new*q1_vw_new - 2*q2_vw_new*q3_vw_new) - (q0_wi_new^2 - q1_wi_new^2 + q2_wi_new^2 - q3_wi_new^2)*(q0_vw_new^2 - q1_vw_new^2 + q2_vw_new^2 - q3_vw_new^2) + (2*q0_wi_new*q3_wi_new - 2*q1_wi_new*q2_wi_new)*(2*q0_vw_new*q3_vw_new + 2*q1_vw_new*q2_vw_new)), -lamda_new*((2*q0_wi_new*q1_wi_new - 2*q2_wi_new*q3_wi_new)*(q0_vw_new^2 - q1_vw_new^2 + q2_vw_new^2 - q3_vw_new^2) - (2*q0_wi_new*q2_wi_new + 2*q1_wi_new*q3_wi_new)*(2*q0_vw_new*q3_vw_new + 2*q1_vw_new*q2_vw_new) + (2*q0_vw_new*q1_vw_new - 2*q2_vw_new*q3_vw_new)*(q0_wi_new^2 - q1_wi_new^2 - q2_wi_new^2 + q3_wi_new^2))]
                              [ -lamda_new*((2*q0_wi_new*q2_wi_new - 2*q1_wi_new*q3_wi_new)*(q0_vw_new^2 - q1_vw_new^2 - q2_vw_new^2 + q3_vw_new^2) - (2*q0_wi_new*q3_wi_new + 2*q1_wi_new*q2_wi_new)*(2*q0_vw_new*q1_vw_new + 2*q2_vw_new*q3_vw_new) + (2*q0_vw_new*q2_vw_new - 2*q1_vw_new*q3_vw_new)*(q0_wi_new^2 + q1_wi_new^2 - q2_wi_new^2 - q3_wi_new^2)),  lamda_new*((2*q0_wi_new*q3_wi_new - 2*q1_wi_new*q2_wi_new)*(2*q0_vw_new*q2_vw_new - 2*q1_vw_new*q3_vw_new) + (2*q0_wi_new*q1_wi_new + 2*q2_wi_new*q3_wi_new)*(q0_vw_new^2 - q1_vw_new^2 - q2_vw_new^2 + q3_vw_new^2) + (2*q0_vw_new*q1_vw_new + 2*q2_vw_new*q3_vw_new)*(q0_wi_new^2 - q1_wi_new^2 + q2_wi_new^2 - q3_wi_new^2)), -lamda_new*((2*q0_wi_new*q1_wi_new - 2*q2_wi_new*q3_wi_new)*(2*q0_vw_new*q1_vw_new + 2*q2_vw_new*q3_vw_new) - (q0_wi_new^2 - q1_wi_new^2 - q2_wi_new^2 + q3_wi_new^2)*(q0_vw_new^2 - q1_vw_new^2 - q2_vw_new^2 + q3_vw_new^2) + (2*q0_wi_new*q2_wi_new + 2*q1_wi_new*q3_wi_new)*(2*q0_vw_new*q2_vw_new - 2*q1_vw_new*q3_vw_new))]];

elseif (param.fusion.camFixPosci == 1)
    H_z_p_relative_pci_new = zeros(3,3);
                
end

% pH(k+m)(p_vw) = delta(z_p_relative) / delta(p_vw_k+m)
if (param.fusion.camFixPosvw == 0)
    H_z_p_relative_pvw_new = [[ -lamda_new*(q0_vw_new^2 + q1_vw_new^2 - q2_vw_new^2 - q3_vw_new^2),          lamda_new*(2*q0_vw_new*q3_vw_new - 2*q1_vw_new*q2_vw_new),         -lamda_new*(2*q0_vw_new*q2_vw_new + 2*q1_vw_new*q3_vw_new)]
                              [         -lamda_new*(2*q0_vw_new*q3_vw_new + 2*q1_vw_new*q2_vw_new), -lamda_new*(q0_vw_new^2 - q1_vw_new^2 + q2_vw_new^2 - q3_vw_new^2),          lamda_new*(2*q0_vw_new*q1_vw_new - 2*q2_vw_new*q3_vw_new)]
                              [          lamda_new*(2*q0_vw_new*q2_vw_new - 2*q1_vw_new*q3_vw_new),         -lamda_new*(2*q0_vw_new*q1_vw_new + 2*q2_vw_new*q3_vw_new), -lamda_new*(q0_vw_new^2 - q1_vw_new^2 - q2_vw_new^2 + q3_vw_new^2)]];

elseif (param.fusion.camFixPosvw == 1)
    H_z_p_relative_pvw_new = zeros(3,3);
                
end

% pH(k+m)(q_vw) = delta(z_p_relative) / delta(q_vw_k+m)
if (param.fusion.camFixQuatvw == 0)
    H_z_p_relative_qvw_new = [[ lamda_new*(2*q0_vw_new*(px_iw_new - px_vw_new + px_ci_new*(q0_wi_new^2 + q1_wi_new^2 - q2_wi_new^2 - q3_wi_new^2) - py_ci_new*(2*q0_wi_new*q3_wi_new - 2*q1_wi_new*q2_wi_new) + pz_ci_new*(2*q0_wi_new*q2_wi_new + 2*q1_wi_new*q3_wi_new)) - 2*q3_vw_new*(py_iw_new - py_vw_new + py_ci_new*(q0_wi_new^2 - q1_wi_new^2 + q2_wi_new^2 - q3_wi_new^2) + px_ci_new*(2*q0_wi_new*q3_wi_new + 2*q1_wi_new*q2_wi_new) - pz_ci_new*(2*q0_wi_new*q1_wi_new - 2*q2_wi_new*q3_wi_new)) + 2*q2_vw_new*(pz_iw_new - pz_vw_new + pz_ci_new*(q0_wi_new^2 - q1_wi_new^2 - q2_wi_new^2 + q3_wi_new^2) - px_ci_new*(2*q0_wi_new*q2_wi_new - 2*q1_wi_new*q3_wi_new) + py_ci_new*(2*q0_wi_new*q1_wi_new + 2*q2_wi_new*q3_wi_new))),  lamda_new*(2*q1_vw_new*(px_iw_new - px_vw_new + px_ci_new*(q0_wi_new^2 + q1_wi_new^2 - q2_wi_new^2 - q3_wi_new^2) - py_ci_new*(2*q0_wi_new*q3_wi_new - 2*q1_wi_new*q2_wi_new) + pz_ci_new*(2*q0_wi_new*q2_wi_new + 2*q1_wi_new*q3_wi_new)) + 2*q2_vw_new*(py_iw_new - py_vw_new + py_ci_new*(q0_wi_new^2 - q1_wi_new^2 + q2_wi_new^2 - q3_wi_new^2) + px_ci_new*(2*q0_wi_new*q3_wi_new + 2*q1_wi_new*q2_wi_new) - pz_ci_new*(2*q0_wi_new*q1_wi_new - 2*q2_wi_new*q3_wi_new)) + 2*q3_vw_new*(pz_iw_new - pz_vw_new + pz_ci_new*(q0_wi_new^2 - q1_wi_new^2 - q2_wi_new^2 + q3_wi_new^2) - px_ci_new*(2*q0_wi_new*q2_wi_new - 2*q1_wi_new*q3_wi_new) + py_ci_new*(2*q0_wi_new*q1_wi_new + 2*q2_wi_new*q3_wi_new))),  lamda_new*(2*q1_vw_new*(py_iw_new - py_vw_new + py_ci_new*(q0_wi_new^2 - q1_wi_new^2 + q2_wi_new^2 - q3_wi_new^2) + px_ci_new*(2*q0_wi_new*q3_wi_new + 2*q1_wi_new*q2_wi_new) - pz_ci_new*(2*q0_wi_new*q1_wi_new - 2*q2_wi_new*q3_wi_new)) - 2*q2_vw_new*(px_iw_new - px_vw_new + px_ci_new*(q0_wi_new^2 + q1_wi_new^2 - q2_wi_new^2 - q3_wi_new^2) - py_ci_new*(2*q0_wi_new*q3_wi_new - 2*q1_wi_new*q2_wi_new) + pz_ci_new*(2*q0_wi_new*q2_wi_new + 2*q1_wi_new*q3_wi_new)) + 2*q0_vw_new*(pz_iw_new - pz_vw_new + pz_ci_new*(q0_wi_new^2 - q1_wi_new^2 - q2_wi_new^2 + q3_wi_new^2) - px_ci_new*(2*q0_wi_new*q2_wi_new - 2*q1_wi_new*q3_wi_new) + py_ci_new*(2*q0_wi_new*q1_wi_new + 2*q2_wi_new*q3_wi_new))), -lamda_new*(2*q3_vw_new*(px_iw_new - px_vw_new + px_ci_new*(q0_wi_new^2 + q1_wi_new^2 - q2_wi_new^2 - q3_wi_new^2) - py_ci_new*(2*q0_wi_new*q3_wi_new - 2*q1_wi_new*q2_wi_new) + pz_ci_new*(2*q0_wi_new*q2_wi_new + 2*q1_wi_new*q3_wi_new)) + 2*q0_vw_new*(py_iw_new - py_vw_new + py_ci_new*(q0_wi_new^2 - q1_wi_new^2 + q2_wi_new^2 - q3_wi_new^2) + px_ci_new*(2*q0_wi_new*q3_wi_new + 2*q1_wi_new*q2_wi_new) - pz_ci_new*(2*q0_wi_new*q1_wi_new - 2*q2_wi_new*q3_wi_new)) - 2*q1_vw_new*(pz_iw_new - pz_vw_new + pz_ci_new*(q0_wi_new^2 - q1_wi_new^2 - q2_wi_new^2 + q3_wi_new^2) - px_ci_new*(2*q0_wi_new*q2_wi_new - 2*q1_wi_new*q3_wi_new) + py_ci_new*(2*q0_wi_new*q1_wi_new + 2*q2_wi_new*q3_wi_new)))]
                              [ lamda_new*(2*q3_vw_new*(px_iw_new - px_vw_new + px_ci_new*(q0_wi_new^2 + q1_wi_new^2 - q2_wi_new^2 - q3_wi_new^2) - py_ci_new*(2*q0_wi_new*q3_wi_new - 2*q1_wi_new*q2_wi_new) + pz_ci_new*(2*q0_wi_new*q2_wi_new + 2*q1_wi_new*q3_wi_new)) + 2*q0_vw_new*(py_iw_new - py_vw_new + py_ci_new*(q0_wi_new^2 - q1_wi_new^2 + q2_wi_new^2 - q3_wi_new^2) + px_ci_new*(2*q0_wi_new*q3_wi_new + 2*q1_wi_new*q2_wi_new) - pz_ci_new*(2*q0_wi_new*q1_wi_new - 2*q2_wi_new*q3_wi_new)) - 2*q1_vw_new*(pz_iw_new - pz_vw_new + pz_ci_new*(q0_wi_new^2 - q1_wi_new^2 - q2_wi_new^2 + q3_wi_new^2) - px_ci_new*(2*q0_wi_new*q2_wi_new - 2*q1_wi_new*q3_wi_new) + py_ci_new*(2*q0_wi_new*q1_wi_new + 2*q2_wi_new*q3_wi_new))), -lamda_new*(2*q1_vw_new*(py_iw_new - py_vw_new + py_ci_new*(q0_wi_new^2 - q1_wi_new^2 + q2_wi_new^2 - q3_wi_new^2) + px_ci_new*(2*q0_wi_new*q3_wi_new + 2*q1_wi_new*q2_wi_new) - pz_ci_new*(2*q0_wi_new*q1_wi_new - 2*q2_wi_new*q3_wi_new)) - 2*q2_vw_new*(px_iw_new - px_vw_new + px_ci_new*(q0_wi_new^2 + q1_wi_new^2 - q2_wi_new^2 - q3_wi_new^2) - py_ci_new*(2*q0_wi_new*q3_wi_new - 2*q1_wi_new*q2_wi_new) + pz_ci_new*(2*q0_wi_new*q2_wi_new + 2*q1_wi_new*q3_wi_new)) + 2*q0_vw_new*(pz_iw_new - pz_vw_new + pz_ci_new*(q0_wi_new^2 - q1_wi_new^2 - q2_wi_new^2 + q3_wi_new^2) - px_ci_new*(2*q0_wi_new*q2_wi_new - 2*q1_wi_new*q3_wi_new) + py_ci_new*(2*q0_wi_new*q1_wi_new + 2*q2_wi_new*q3_wi_new))),  lamda_new*(2*q1_vw_new*(px_iw_new - px_vw_new + px_ci_new*(q0_wi_new^2 + q1_wi_new^2 - q2_wi_new^2 - q3_wi_new^2) - py_ci_new*(2*q0_wi_new*q3_wi_new - 2*q1_wi_new*q2_wi_new) + pz_ci_new*(2*q0_wi_new*q2_wi_new + 2*q1_wi_new*q3_wi_new)) + 2*q2_vw_new*(py_iw_new - py_vw_new + py_ci_new*(q0_wi_new^2 - q1_wi_new^2 + q2_wi_new^2 - q3_wi_new^2) + px_ci_new*(2*q0_wi_new*q3_wi_new + 2*q1_wi_new*q2_wi_new) - pz_ci_new*(2*q0_wi_new*q1_wi_new - 2*q2_wi_new*q3_wi_new)) + 2*q3_vw_new*(pz_iw_new - pz_vw_new + pz_ci_new*(q0_wi_new^2 - q1_wi_new^2 - q2_wi_new^2 + q3_wi_new^2) - px_ci_new*(2*q0_wi_new*q2_wi_new - 2*q1_wi_new*q3_wi_new) + py_ci_new*(2*q0_wi_new*q1_wi_new + 2*q2_wi_new*q3_wi_new))),  lamda_new*(2*q0_vw_new*(px_iw_new - px_vw_new + px_ci_new*(q0_wi_new^2 + q1_wi_new^2 - q2_wi_new^2 - q3_wi_new^2) - py_ci_new*(2*q0_wi_new*q3_wi_new - 2*q1_wi_new*q2_wi_new) + pz_ci_new*(2*q0_wi_new*q2_wi_new + 2*q1_wi_new*q3_wi_new)) - 2*q3_vw_new*(py_iw_new - py_vw_new + py_ci_new*(q0_wi_new^2 - q1_wi_new^2 + q2_wi_new^2 - q3_wi_new^2) + px_ci_new*(2*q0_wi_new*q3_wi_new + 2*q1_wi_new*q2_wi_new) - pz_ci_new*(2*q0_wi_new*q1_wi_new - 2*q2_wi_new*q3_wi_new)) + 2*q2_vw_new*(pz_iw_new - pz_vw_new + pz_ci_new*(q0_wi_new^2 - q1_wi_new^2 - q2_wi_new^2 + q3_wi_new^2) - px_ci_new*(2*q0_wi_new*q2_wi_new - 2*q1_wi_new*q3_wi_new) + py_ci_new*(2*q0_wi_new*q1_wi_new + 2*q2_wi_new*q3_wi_new)))]
                              [ lamda_new*(2*q1_vw_new*(py_iw_new - py_vw_new + py_ci_new*(q0_wi_new^2 - q1_wi_new^2 + q2_wi_new^2 - q3_wi_new^2) + px_ci_new*(2*q0_wi_new*q3_wi_new + 2*q1_wi_new*q2_wi_new) - pz_ci_new*(2*q0_wi_new*q1_wi_new - 2*q2_wi_new*q3_wi_new)) - 2*q2_vw_new*(px_iw_new - px_vw_new + px_ci_new*(q0_wi_new^2 + q1_wi_new^2 - q2_wi_new^2 - q3_wi_new^2) - py_ci_new*(2*q0_wi_new*q3_wi_new - 2*q1_wi_new*q2_wi_new) + pz_ci_new*(2*q0_wi_new*q2_wi_new + 2*q1_wi_new*q3_wi_new)) + 2*q0_vw_new*(pz_iw_new - pz_vw_new + pz_ci_new*(q0_wi_new^2 - q1_wi_new^2 - q2_wi_new^2 + q3_wi_new^2) - px_ci_new*(2*q0_wi_new*q2_wi_new - 2*q1_wi_new*q3_wi_new) + py_ci_new*(2*q0_wi_new*q1_wi_new + 2*q2_wi_new*q3_wi_new))),  lamda_new*(2*q3_vw_new*(px_iw_new - px_vw_new + px_ci_new*(q0_wi_new^2 + q1_wi_new^2 - q2_wi_new^2 - q3_wi_new^2) - py_ci_new*(2*q0_wi_new*q3_wi_new - 2*q1_wi_new*q2_wi_new) + pz_ci_new*(2*q0_wi_new*q2_wi_new + 2*q1_wi_new*q3_wi_new)) + 2*q0_vw_new*(py_iw_new - py_vw_new + py_ci_new*(q0_wi_new^2 - q1_wi_new^2 + q2_wi_new^2 - q3_wi_new^2) + px_ci_new*(2*q0_wi_new*q3_wi_new + 2*q1_wi_new*q2_wi_new) - pz_ci_new*(2*q0_wi_new*q1_wi_new - 2*q2_wi_new*q3_wi_new)) - 2*q1_vw_new*(pz_iw_new - pz_vw_new + pz_ci_new*(q0_wi_new^2 - q1_wi_new^2 - q2_wi_new^2 + q3_wi_new^2) - px_ci_new*(2*q0_wi_new*q2_wi_new - 2*q1_wi_new*q3_wi_new) + py_ci_new*(2*q0_wi_new*q1_wi_new + 2*q2_wi_new*q3_wi_new))), -lamda_new*(2*q0_vw_new*(px_iw_new - px_vw_new + px_ci_new*(q0_wi_new^2 + q1_wi_new^2 - q2_wi_new^2 - q3_wi_new^2) - py_ci_new*(2*q0_wi_new*q3_wi_new - 2*q1_wi_new*q2_wi_new) + pz_ci_new*(2*q0_wi_new*q2_wi_new + 2*q1_wi_new*q3_wi_new)) - 2*q3_vw_new*(py_iw_new - py_vw_new + py_ci_new*(q0_wi_new^2 - q1_wi_new^2 + q2_wi_new^2 - q3_wi_new^2) + px_ci_new*(2*q0_wi_new*q3_wi_new + 2*q1_wi_new*q2_wi_new) - pz_ci_new*(2*q0_wi_new*q1_wi_new - 2*q2_wi_new*q3_wi_new)) + 2*q2_vw_new*(pz_iw_new - pz_vw_new + pz_ci_new*(q0_wi_new^2 - q1_wi_new^2 - q2_wi_new^2 + q3_wi_new^2) - px_ci_new*(2*q0_wi_new*q2_wi_new - 2*q1_wi_new*q3_wi_new) + py_ci_new*(2*q0_wi_new*q1_wi_new + 2*q2_wi_new*q3_wi_new))),  lamda_new*(2*q1_vw_new*(px_iw_new - px_vw_new + px_ci_new*(q0_wi_new^2 + q1_wi_new^2 - q2_wi_new^2 - q3_wi_new^2) - py_ci_new*(2*q0_wi_new*q3_wi_new - 2*q1_wi_new*q2_wi_new) + pz_ci_new*(2*q0_wi_new*q2_wi_new + 2*q1_wi_new*q3_wi_new)) + 2*q2_vw_new*(py_iw_new - py_vw_new + py_ci_new*(q0_wi_new^2 - q1_wi_new^2 + q2_wi_new^2 - q3_wi_new^2) + px_ci_new*(2*q0_wi_new*q3_wi_new + 2*q1_wi_new*q2_wi_new) - pz_ci_new*(2*q0_wi_new*q1_wi_new - 2*q2_wi_new*q3_wi_new)) + 2*q3_vw_new*(pz_iw_new - pz_vw_new + pz_ci_new*(q0_wi_new^2 - q1_wi_new^2 - q2_wi_new^2 + q3_wi_new^2) - px_ci_new*(2*q0_wi_new*q2_wi_new - 2*q1_wi_new*q3_wi_new) + py_ci_new*(2*q0_wi_new*q1_wi_new + 2*q2_wi_new*q3_wi_new)))]];

elseif (param.fusion.camFixQuatvw == 1)
    H_z_p_relative_qvw_new = zeros(3,4);
                
end

% pH_relative
H_Pos_Relative = [H_z_p_relative_qwi_old, zeros(3,3), H_z_p_relative_piw_old, zeros(3,14), H_z_p_relative_lamda_old, H_z_p_relative_pci_old, zeros(3,4), H_z_p_relative_pvw_old, H_z_p_relative_qvw_old, H_z_p_relative_qwi_new, zeros(3,3), H_z_p_relative_piw_new, zeros(3,14), H_z_p_relative_lamda_new, H_z_p_relative_pci_new, zeros(3,4), H_z_p_relative_pvw_new, H_z_p_relative_qvw_new];

